#include "robothelper.h"
using namespace std ;
using namespace Eigen;

#include <QDebug>
#include <QTimer>
#include <boost/scope_exit.hpp>


static Config _robotConf("etc/communication/robot.sys");

RobotHelper::RobotHelper()
{
    //recodeBug1();
    // SelectRobot();

}




void RobotHelper::recodeBug1()
{
    std::string str = "0 0 0 0 0 0 ";
    RobotPos f = {0,0,0,0,0,0,FLANGE};
    bool ret = str2Robot( str,f);
    qDebug()<<"......recodeBug1";

}

void RobotHelper::PauseMotion(bool state)
{
    if(state)
        _robot->stopMove(RobotError::FORCESTOPMOVING) ;
    else{
        ///
    }
}

void RobotHelper::CriticalStop(bool state)
{
    if(state)
        _robot->stopMove(SERVICESTOP) ;
    else{
        ///
    }
}

void RobotHelper::RelativeMove(RobotPos pos,int speed )
{
    auto th = std::thread(&RobotHelper::_relativeMove,this, pos,speed);
    th.detach();
}

void RobotHelper::ABSMove(RobotPos target, int speed)
{
    auto th = std::thread(&RobotHelper::_absMove,this, target,speed);
    th.detach();
}


void RobotHelper::OutSilk()
{

}

void RobotHelper::InSilk()
{

}

void RobotHelper::ConnectToRobot()
{
    cout << "0:SelectRobot"<< endl;
    /// 选择机器人
    SelectRobot();
    cout << "1:ReadConfig"<< endl;
    /// 1.连接网络端口
    ReadConfig();
    cout << "2:StartUDP"<< endl;
    StartUDP();
    cout << "3:Sleep"<< endl;

    std::this_thread::sleep_for( std::chrono::milliseconds(1000) ) ;
    /// 2.启动机器人事件循环
    UDPHelper::GetInstance()->p()->WriteBuff("start");

    _allowMove = true;
    ///
    std::this_thread::sleep_for( std::chrono::milliseconds(500) ) ;
    GetError();

    std::thread([this]{

        RobotPos nullPos = {0,0,0,0,0,0,FLANGE};
        weldInfo nu_ptr = {0,0,0,0,0,0,0,0,0};
        int  goIndex = 0;

        while (_allowMove) {
            //std::this_thread::sleep_for(1_ms);  /// 等待
            RobotPos temp = {0,0,0,0,0,0,FLANGE};
            while (!zeroLock.load());//用于控制进入
            if(_threadHandle.empty()){
                goIndex = 0;
                continue;
            }

            auto cur = _robot->getCurrPos( RobotTCP::FLANGE );
            auto &_ptr0 =  _threadHandle.at(0);
            if(1 < abs(_sys_config.get<double>("robot.speed")- std::get<3>(_ptr0))){
                _sys_config.put("robot.speed", std::get<3>(_ptr0));
                _sys_config.sync();
            }

            if(!std::get<4>(_ptr0)){
                temp.a = std::get<0>(_ptr0) * T_P_ONCE_MAX / 2;
                temp.b = std::get<1>(_ptr0) * T_P_ONCE_MAX / 2;
                temp.c = std::get<2>(_ptr0) * T_P_ONCE_MAX / 2;
            }else{
                temp.x = std::get<0>(_ptr0) * T_P_ONCE_MAX;
                temp.y = std::get<1>(_ptr0) * T_P_ONCE_MAX;
                temp.z = std::get<2>(_ptr0) * T_P_ONCE_MAX;
            }
            _robot->sendWeldPoss(goIndex,temp+cur,nullPos,nullPos,0,0,nu_ptr);

            _threadHandle.pop_front();
            goIndex++;
        }
    }).detach();

}

void RobotHelper::SetCoordi()
{

}

void RobotHelper::SetTCP()
{

}

void RobotHelper::GetError()
{
    // 获取机器人故障状态
    if(_robot->rgsoStatus < 0)
    {
        cout<<"get error info!"<<endl;
//        sendBuf = "error";
//        sendFlag = true;
    }

}

void RobotHelper::CloseRobot()
{
    _allowMove = false;
    UDPHelper::GetInstance()->p()->DisConnectToHost();
    UDPHelper::GetInstance()->p()->WriteBuff("stop");
}

RobotPos RobotHelper::getCurrentPos(){
    return _robot->getCurrPos();
}


void RobotHelper::ProcessContinousPos(double px, double py, double pz, int speed, bool motionType)
{
    // std::cout<<"---->"<<_ptr0.px<<"\t"<<_ptr0.py<<"\t"<<_ptr0.pz <<std::endl;
    if(0.01 > abs(px) && 0.01 > abs(py) && 0.01 > abs(pz)){
        zeroLock = false;
        _threadHandle.clear();
    }else{
        zeroLock = true;
        _threadHandle.push_back(HandlerReal(px,py,pz,speed,motionType));
    }

}

void RobotHelper::StartUDP()
{
    UDPHelper::GetInstance()->p()->ConnectToHost();
}

void RobotHelper::ReadConfig()
{
    switch ((ROBOT::Tag_COM_TYPE)_robotConf.get<int>("comType")) {
    case ROBOT::T_UDP:
        UDPHelper::GetInstance()->p()->SetIP(_robotConf.get<string>("UDP.ip").c_str(),_robotConf.get<int>("UDP.port"));
        break;
    case ROBOT::T_TCP:
        UDPHelper::GetInstance()->p()->SetIP(_robotConf.get<string>("TCP.ip").c_str(),_robotConf.get<int>("TCP.port"));
        break;
    default:
        break;
    }

}

void RobotHelper::_relativeMove(RobotPos pos,int speed)
{
    RobotPos relativePos = pos;
    try{
        if(1 < abs(_sys_config.get<double>("robot.speed")-speed)){
            _sys_config.put("robot.speed", speed);
            _sys_config.sync();
        }

        if ( !_sys_config.get<bool>("robot.bEular")) {
            _robot->relativeMove_axis( {relativePos},
                                       true,
                                       _bWeldingOn) ;
        }
        else {
            _robot->relativeMove( relativePos, true, _bWeldingOn ) ;
        }

    }catch( const boost::exception &e ) {
        DUMPERROR(e);
    }
}

void RobotHelper::_absMove(RobotPos target, int speed)
{
    try{
        if(1 < abs(_sys_config.get<double>("robot.speed")-speed)){
            _sys_config.put("robot.speed", speed);
            _sys_config.sync();
        }
        _robot->absoluteMove(target);
    }catch( const boost::exception &e ) {
        DUMPERROR(e) ;
    }
}
